#!/bin/bash

# 设置颜色代码
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
RED='\033[0;31m'
NC='\033[0m' # No Color

# 设置ROS环境
source /opt/ros/noetic/setup.bash
source /root/catkin_ws/devel/setup.bash

# 默认参数
MAP_FILE="/root/catkin_ws/src/auto_navigation/maps/kitti_best_map.pcd"
BAG_FILE=""
BAG_RATE=0.3
BAG_START=0.0
RVIZ=true

# 显示帮助信息
function show_help {
    echo -e "${BLUE}=== KITTI数据集NDT定位运行脚本 ===${NC}"
    echo "用法: $0 [选项]"
    echo ""
    echo "选项:"
    echo "  -h, --help                显示帮助信息"
    echo "  -m, --map FILE            指定地图文件路径 (默认: $MAP_FILE)"
    echo "  -b, --bag FILE            指定bag文件路径 (必需)"
    echo "  -r, --rate RATE           指定bag文件回放速率 (默认: $BAG_RATE)"
    echo "  -s, --start TIME          指定bag文件起始时间 (默认: $BAG_START)"
    echo "  --no-rviz                 不启动RViz可视化"
    echo ""
    echo "示例:"
    echo "  $0 -b /path/to/kitti.bag -m /path/to/map.pcd -r 0.5"
    echo ""
}

# 解析命令行参数
while [[ $# -gt 0 ]]; do
    key="$1"
    case $key in
        -h|--help)
            show_help
            exit 0
            ;;
        -m|--map)
            MAP_FILE="$2"
            shift
            shift
            ;;
        -b|--bag)
            BAG_FILE="$2"
            shift
            shift
            ;;
        -r|--rate)
            BAG_RATE="$2"
            shift
            shift
            ;;
        -s|--start)
            BAG_START="$2"
            shift
            shift
            ;;
        --no-rviz)
            RVIZ=false
            shift
            ;;
        *)
            echo -e "${RED}错误: 未知选项 $1${NC}"
            show_help
            exit 1
            ;;
    esac
done

# 检查必需参数
if [ -z "$BAG_FILE" ]; then
    echo -e "${RED}错误: 必须指定bag文件路径${NC}"
    show_help
    exit 1
fi

# 检查文件是否存在
if [ ! -f "$MAP_FILE" ]; then
    echo -e "${RED}错误: 地图文件不存在: $MAP_FILE${NC}"
    exit 1
fi

if [ ! -f "$BAG_FILE" ]; then
    echo -e "${RED}错误: bag文件不存在: $BAG_FILE${NC}"
    exit 1
fi

# 显示配置信息
echo -e "${BLUE}=== KITTI数据集NDT定位配置 ===${NC}"
echo -e "地图文件: ${YELLOW}$MAP_FILE${NC}"
echo -e "bag文件: ${YELLOW}$BAG_FILE${NC}"
echo -e "回放速率: ${YELLOW}$BAG_RATE${NC}"
echo -e "起始时间: ${YELLOW}$BAG_START${NC}"
echo -e "启用RViz: ${YELLOW}$RVIZ${NC}"

# 构建roslaunch命令
LAUNCH_CMD="roslaunch auto_navigation kitti_ndt_localization.launch"
LAUNCH_CMD="$LAUNCH_CMD map_file:=$MAP_FILE"
LAUNCH_CMD="$LAUNCH_CMD bag_file:=$BAG_FILE"
LAUNCH_CMD="$LAUNCH_CMD bag_rate:=$BAG_RATE"
LAUNCH_CMD="$LAUNCH_CMD bag_start:=$BAG_START"
LAUNCH_CMD="$LAUNCH_CMD rviz:=$RVIZ"

# 启动NDT定位
echo -e "${GREEN}启动KITTI数据集NDT定位...${NC}"
echo -e "${YELLOW}按Ctrl+C终止${NC}"
$LAUNCH_CMD

echo -e "${BLUE}=== KITTI数据集NDT定位已完成 ===${NC}" 